#include "main.h"
#include "gpio.h"
#include "cmsis_os2.h"

#include "oslib.h"
#include "vesc_can.h"
#include "dji2_0.h"
#include "shooter.h"
#include "shooter_clutch.h"
#include "laser.h"
#include "boxReader.h"
#include "claw_signal.h"
#include "../race_tasks/race_tasks.h"
#include "lightband.h"

#include "toolBoxScope.h"
/*-define---------------------------------------*/
// #define TEST_SHOOTER
// #define TEST_PROCESS
#define SHOOTER_LASER
#define JUST_FOR_DEBUG
#define ENCODER_RELATIVE

#define USE_FORCE 1
#define USE_PITCH 1
#define USE_YAW 1
#define USE_AUTOAIM 1

#define CTRL_INTV 10
#define PMOTOR_HOM_VEL 400
#define PMOTOR_HOM_CUR 800
#define PMOTOR_POS_VEL 600

#define YMOTOR_HOM_VEL 30
#define YMOTOR_HOM_CUR 5000
#define YMOTOR_POS_VEL 50

#define FMOTOR_MAX_VEL 250
#define FMOTOR_NMAX_VEL -75

#define LOAD_POS_FORCE 800
#define LOAD_POS_PITCH -1800000
// #define LOAD_POS_YAW    -7500
#define LOAD_POS_YAW -60000
// #define MAX_DUTY        0.95
// #define NMAX_DUTY       -0.15

#define INIT_FORCE_POS 354

/* 来自内部的事件 */
#define SHOOTER_EVENT_IN_FORCE (1UL << 0)
#define SHOOTER_EVENT_IN_PITCH (1UL << 1)
#define SHOOTER_EVENT_IN_YAW (1UL << 2)
#define SHOOTER_EVENT_IN_ALL (SHOOTER_EVENT_IN_FORCE | SHOOTER_EVENT_IN_PITCH | SHOOTER_EVENT_IN_YAW)
/* 来自外部的事件 */
#define SHOOTER_EVENT_OUT_LOAD (1UL << 4)
#define SHOOTER_EVENT_OUT_SHOOT (1UL << 5)
#define SHOOTER_EVENT_OUT_AIM (1UL << 6)
#define SHOOTER_EVENT_OUT_NOTTAKE (1UL << 7) // 不处于取箭状态
/* 调试使用的事件 */
#define SHOOTER_EVENT_TEST_START (1UL << 8)
#define SHOOTER_EVENT_TEST_RESET (1UL << 9)

#define __max(a, b) (((a) > (b)) ? (a) : (b))
#define __min(a, b) (((a) < (b)) ? (a) : (b))

/*-typedefs-------------------------------------*/

/*-static variables-------------------------------*/
shooter_t __shooter1, *shooter1 = &__shooter1;

static shooter_status_t pre_adjust_param[] = {
    {.force = 860, .pitch = -600000, .yaw = -97000}, // 起点I型壶
    {.force = LOAD_POS_FORCE, .pitch = LOAD_POS_PITCH, .yaw = LOAD_POS_YAW},
};
static int pre_adjust_param_count = sizeof(pre_adjust_param)/sizeof(pre_adjust_param[0]);

const osThreadAttr_t shooter_task_attrs = {
    .name = "ShooterTask",
    .priority = osPriorityNormal2,
    .stack_size = 192 * 4,
};

/*-static functions-----------------------------*/

static int shooter_now_force(shooter_handle_t sht)
{
#ifdef ENCODER_RELATIVE
    return sht->now_stat.force;
#else
    return -(sht->now_round_cnt * 360 + sht->now_force_pos - INIT_FORCE_POS);
#endif
}

// static float shooter_force_control(shooter_handle_t sht)
// {
//     float now_err = sht->tar_stat.force - sht->now_stat.force;
//     float last_err = sht->force_pid.last_err;
//     sht->force_pid.last_err = now_err;
//     // 只有到达一定范围内才会开始i的积累
//     if (now_err <= sht->force_pid.i_range_p && now_err >= sht->force_pid.i_range_n) {
//         if (now_err <= 0) {
//             // sht->force_pid.i = 0;
//             sht->force_pid.i += now_err * sht->force_pid.KI_N;
//             sht->force_pid.i = __max(sht->force_pid.i, sht->force_pid.i_max_n);
//         }
//         else {
//             sht->force_pid.i += now_err * sht->force_pid.KI_P;
//             sht->force_pid.i = __min(sht->force_pid.i, sht->force_pid.i_max_p);
//         }
//     }
//     else {
//         sht->force_pid.i = 0;
//     }

//     float duty;
//     duty = sht->force_pid.Kp * now_err - sht->force_pid.Kp * (now_err - last_err) + sht->force_pid.i;
//     if (duty > MAX_DUTY) duty = MAX_DUTY;
//     else if (duty < NMAX_DUTY) duty = NMAX_DUTY;
//     comm_can_set_duty(sht->vesc_id, duty);

//     return duty;
// }

static float shooter_force_control(shooter_handle_t sht)
{
    float now_err = sht->tar_stat.force - shooter_now_force(sht);
    float last_err = sht->force_pid.last_err;
    sht->force_pid.last_err = now_err;

    float vel;
    vel = sht->force_pid.Kp * now_err - sht->force_pid.Kp * (now_err - last_err);
    vel = __min(vel, FMOTOR_MAX_VEL);
    vel = __max(vel, FMOTOR_NMAX_VEL);
    // 由于方向相反, 所以速度添加一个负号
    dji_VelCtrl(sht->hcan, sht->dji_board_id, sht->dji_fmotor_id, (int32_t)-vel);

    return vel;
}

static void shooter_force_control_lock(shooter_handle_t sht)
{
    // 发送锁速度环消息
    dji_VelCtrl(sht->hcan, sht->dji_board_id, sht->dji_fmotor_id, 0);
}

static void shooter_pitch_control(shooter_handle_t sht)
{
    dji_PosCtrl(sht->hcan, sht->dji_board_id, sht->dji_pmotor_id, sht->tar_stat.pitch);
}

static void shooter_yaw_control(shooter_handle_t sht)
{
    if (sht->state != SHOOTER_STATE_AIMI)
    {
        dji_PosCtrl(sht->hcan, sht->dji_board_id, sht->dji_ymotor_id, sht->tar_stat.yaw);
    }
}

static int shooter_force_check(shooter_handle_t sht)
{
    static int arrive_count = 0;
    if (abs(sht->tar_stat.force - shooter_now_force(sht)) <= 1)
    {
        arrive_count++;
    }
    else
    {
        arrive_count = 0;
    }
    if (arrive_count >= 50)
    {
        arrive_count = 50;
        // osEventFlagsSet(sht->events, SHOOTER_EVENT_IN_FORCE);
        return 1;
    }
    return 0;
}

static int shooter_pitch_check(shooter_handle_t sht)
{
    // uint32_t evt = osEventFlagsGet(sht->events);
    // if (evt & SHOOTER_EVENT_IN_PITCH) {
    //     return 1;
    // }
    // return 0;
    return (sht->now_stat.pitch == sht->tar_stat.pitch);
}

static int shooter_yaw_check(shooter_handle_t sht)
{
    uint32_t evt = osEventFlagsGet(sht->events);
    if (evt & SHOOTER_EVENT_IN_YAW)
    {
        return 1;
    }
    return 0;
}

static const char *just_for_debug_output(shooter_state_t state)
{
    switch (state)
    {
    case SHOOTER_STATE_START:
        return "[START]";
        break;
    case SHOOTER_STATE_ROTATE:
        return "[ROTATE]";
        break;
    case SHOOTER_STATE_LOAD:
        return "[LOAD]";
        break;
    case SHOOTER_STATE_READY:
        return "[READY]";
        break;
    case SHOOTER_STATE_AIMI:
        return "[AIMI]";
        break;
    case SHOOTER_STATE_ADJUST:
        return "[ADJUST]";
        break;
    case SHOOTER_STATE_SHOOT:
        return "[SHOOT]";
        break;
    default:
        return "[NONE]";
        break;
    }
}

static int shooter_check_and_control(shooter_handle_t sht)
{
    uint32_t ret = 1;
#if USE_FORCE
    float duty = 0;
    if (!shooter_force_check(sht))
    {
        duty = shooter_force_control(sht);
        ret = 0;
    }
    else
    {
        shooter_force_control_lock(sht);
    }
#endif
#if USE_PITCH
    if (!shooter_pitch_check(sht))
    {
        shooter_pitch_control(sht);
        ret = 0;
    }
#endif
#if USE_YAW
    if (sht->state == SHOOTER_STATE_ROTATE || (sht->mode == SHOOTER_MODE_FIXED && sht->state == SHOOTER_STATE_ADJUST))
    {
        if (!shooter_yaw_check(sht))
        {
            shooter_yaw_control(sht);
            ret = 0;
        }
    }
#endif
#ifdef JUST_FOR_DEBUG
    /* FIXME: Just For Debug */
    static int tempcount = 0;
    tempcount++;
    if (tempcount % 10 == 0)
        uprintf("%s\t%d\t%.2f\t%.1f\r\n", just_for_debug_output(sht->state), shooter_now_force(sht), duty, laser_get_dist(shooter_laser));
    /* FIXME: Just For Debug */
#endif
    return ret;
}

static void shooter_clear_pid(shooter_handle_t sht)
{
    sht->force_pid.i = 0;
    sht->force_pid.last_err = 0;
}

static void shooter_clear_event(shooter_handle_t sht)
{
    osEventFlagsClear(sht->events, SHOOTER_EVENT_IN_ALL);
}

static void shooter_task(void *argument)
{
    shooter_handle_t sht = (shooter_handle_t)argument;
    uint32_t evt = 0, ret = 0;
    static shooter_status_t stat;
    /* FIXME: Just For Debug */
    // for (;;)
    // {
    //     // uprintf("dist=%.2f\n", laser_get_dist(sht->laser));
    //     float dist = laser_get_dist(sht->laser);
    //     toolBox_scope(&dist, 1);
    //     osDelay(5);
    // }
    /* FIXME: Just For Debug */
    // 第一个状态机, 完成初始化
#if (USE_PITCH || USE_YAW)
    for (;;)
    {
        evt = osEventFlagsGet(sht->events);
#if USE_PITCH
        if (!(evt & SHOOTER_EVENT_IN_PITCH))
        {
            dji_Homing(sht->hcan, sht->dji_board_id, sht->dji_pmotor_id, PMOTOR_HOM_VEL, PMOTOR_HOM_CUR);
        }
#endif
#if USE_YAW
        if (!(evt & SHOOTER_EVENT_IN_YAW))
        {
            dji_Homing(sht->hcan, sht->dji_board_id, sht->dji_ymotor_id, YMOTOR_HOM_VEL, YMOTOR_HOM_CUR);
        }
#endif
        uint32_t wait_evt = 0;
#if USE_PITCH
        wait_evt |= SHOOTER_EVENT_IN_PITCH;
#endif
#if USE_YAW
        wait_evt |= SHOOTER_EVENT_IN_YAW;
#endif
        evt = osEventFlagsWait(sht->events, wait_evt, osFlagsWaitAll, 2000);
        if (evt > osStatusReserved)
        {
            if (evt != (uint32_t)osErrorTimeout)
            {
                uprintf("[Error] Shooter init evt=%u\r\n", evt);
            }
            else
            {
                uprintf("[Init] Retrying...\r\n");
            }
        }
        else if (evt & (wait_evt))
        {
            break;
        }
    }
#endif
#if USE_PITCH
    dji_NoInitPosCfg(sht->hcan, sht->dji_board_id, sht->dji_pmotor_id, PMOTOR_POS_VEL);
#endif
#if USE_YAW
    dji_NoInitPosCfg(sht->hcan, sht->dji_board_id, sht->dji_ymotor_id, YMOTOR_POS_VEL);
#endif
    sht->init = 1;
    uprintf("[Init] Finish\n");

    // 第二个状态机
    sht->state = SHOOTER_STATE_START;
    for (;;)
    {
        switch (sht->state)
        {
        case SHOOTER_STATE_START: // 流程结束->流程开始
            // TODO: 查询标志位, 检查取箭机构是否正在取箭
            evt = osEventFlagsWait(sht->events, SHOOTER_EVENT_OUT_NOTTAKE, osFlagsWaitAny | osFlagsNoClear, osWaitForever);
            // 如果正在取箭, 射箭机构不能立刻回到上箭位
            osMutexAcquire(sht->mutex, osWaitForever);
#ifdef ENCODER_RELATIVE
            sht->now_stat.force = 0;
#else
            sht->now_round_cnt = 0;
#endif
            osMutexRelease(sht->mutex);
#if USE_YAW
            dji_NoInitPosCfg(sht->hcan, sht->dji_board_id, sht->dji_ymotor_id, 50);
#endif
            sht->tar_stat.force = LOAD_POS_FORCE;
            sht->tar_stat.pitch = LOAD_POS_PITCH;
            sht->tar_stat.yaw = LOAD_POS_YAW;
            shooter_clutch_set_lock(&sht->clutch);
            uprintf("[Start]\n");
            lb_set_light(3, LB_COLOR_WHITE, LB_BLINK_KEEP);
            sht->state = SHOOTER_STATE_ROTATE;
            break;
        case SHOOTER_STATE_ROTATE: // 流程开始->上箭位
            if (shooter_check_and_control(sht))
            {
                // 发送上箭请求
                Signal_MoveClaw();
                uprintf("[Load]\n");
                lb_set_light(3, LB_COLOR_YELLOW, LB_BLINK_KEEP);
                sht->state = SHOOTER_STATE_LOAD;
            }
            else
            {
                osDelay(CTRL_INTV);
            }
            break;
        case SHOOTER_STATE_LOAD: // 上箭位->上箭完成
            evt = osEventFlagsWait(sht->events, SHOOTER_EVENT_OUT_LOAD, osFlagsWaitAny, CTRL_INTV);
            if (evt > osStatusReserved)
            {
                if (evt == (uint32_t)osErrorTimeout)
                {
                    shooter_check_and_control(sht);
                }
            }
            else if (evt & SHOOTER_EVENT_OUT_LOAD)
            {
                uprintf("[Ready]\n");
                // 闪灯提示瞄准
                lb_set_light(3, LB_COLOR_ORANGE, LB_BLINK_SLOW);
                sht->state = SHOOTER_STATE_READY;
            }
            break;
        case SHOOTER_STATE_READY: // 上箭完成->瞄准
            evt = osEventFlagsWait(sht->events, SHOOTER_EVENT_OUT_AIM, osFlagsWaitAny, CTRL_INTV);
            if (evt == (uint32_t)osErrorTimeout)
            {
                shooter_check_and_control(sht);
            }
            else if (evt < osStatusReserved && (evt & SHOOTER_EVENT_OUT_AIM) == SHOOTER_EVENT_OUT_AIM)
            {
                shooter_clear_event(sht);
                shooter_clear_pid(sht);
                // TODO: 预调整, 根据当前的瞄准编号(全局变量)进行预调整
#ifdef TEST_PROCESS
                /* FIXME 临时设定的预调整参数 */
                sht->tar_stat.force = 750;
                sht->tar_stat.pitch = -1500000;
                sht->tar_stat.yaw = -50000;
                /* FIXME 临时设定的预调整参数 */
#else
                sht->tar_stat = pre_adjust_param[__min(sht->point, pre_adjust_param_count-1)];
#endif
                shooter_yaw_control(sht);
                if (sht->mode == SHOOTER_MODE_DYNAMIC) {
#if USE_YAW
#if USE_AUTOAIM
                // 开始瞄准
                osSemaphoreRelease(TripodTokenSema);
                lb_set_light(3, LB_COLOR_RED, LB_BLINK_KEEP);   // 红灯表示手动模式
#else
                osEventFlagsSet(sht->events, SHOOTER_EVENT_IN_YAW);
#endif
#endif
                uprintf("[Aim]\r\n");
                sht->state = SHOOTER_STATE_AIMI;
                }
                else {
                    sht->state = SHOOTER_STATE_ADJUST;
                }
            }
            break;
        case SHOOTER_STATE_AIMI: // 瞄准->准备发射
            // evt = osEventFlagsWait(sht->events, SHOOTER_EVENT_IN_YAW, osFlagsWaitAny, CTRL_INTV);
            // if (evt == (uint32_t)osErrorTimeout) {
            //     shooter_check_and_control(sht);
            // }
            // else if ((evt & SHOOTER_EVENT_IN_YAW) == SHOOTER_EVENT_IN_YAW) {
            // 设置参数
            //     sht->state = SHOOTER_STATE_ADJUST;
            // }
            ret = osMessageQueueGet(sht->queue, &stat, NULL, CTRL_INTV);
            if (ret == (uint32_t)osErrorTimeout)
            {
                shooter_check_and_control(sht);
            }
            else if (ret == osOK)
            {
                // TODO: 检查参数是否符合要求
                sht->tar_stat = stat;
                shooter_clear_event(sht);
                shooter_clear_pid(sht);
                uprintf("[ADJUST]\n");
                sht->state = SHOOTER_STATE_ADJUST;
            }
            break;
        case SHOOTER_STATE_ADJUST:
            evt = osEventFlagsGet(sht->events);
            if ((evt & SHOOTER_EVENT_OUT_AIM) == SHOOTER_EVENT_OUT_AIM)
            {
                // 重新瞄准+预调整
                sht->state = SHOOTER_STATE_READY;
            }
            if (shooter_check_and_control(sht))
            {
                uprintf("[SHOOT]\n");
                // TODO: 开始闪灯, 提示操作手就绪
                lb_set_light(3, LB_COLOR_GREEN, LB_BLINK_SLOW);
                sht->state = SHOOTER_STATE_SHOOT;
            }
            else
            {
                osDelay(CTRL_INTV);
            }
            break;
        case SHOOTER_STATE_SHOOT: // 发射准备->流程结束
            evt = osEventFlagsGet(sht->events);
            if ((evt & SHOOTER_EVENT_OUT_AIM) == SHOOTER_EVENT_OUT_AIM)
            {
                sht->state = SHOOTER_STATE_READY;
            }
            evt = osEventFlagsWait(sht->events, SHOOTER_EVENT_OUT_SHOOT, osFlagsWaitAny, CTRL_INTV);
            if (evt > osStatusReserved)
            {
                if (evt == (uint32_t)osErrorTimeout)
                {
                    shooter_check_and_control(sht);
                }
            }
            else if (evt & SHOOTER_EVENT_OUT_SHOOT)
            {
                shooter_clutch_set_unlock(&sht->clutch);
                uprintf("[DONE]\n");
                lb_set_light(3, LB_COLOR_BLACK, LB_BLINK_KEEP);
                osDelay(800);
                shooter_clear_event(sht);
                shooter_clear_pid(sht);
                sht->state = SHOOTER_STATE_START;
                // 告诉顶层状态机, 一轮射箭结束, 选择下一个射箭点
                RaceStateMachine.arrow_shooted = 1;
            }
            break;
        default:
            uprintf("[ERROR] Unknown shooter state\n");
            break;
        }
    }
}

/*-global functions------------------------------*/
/* 初始化 */
/**
 * @brief 射箭结构初始化
 * 
 * @param sht           射箭机构结构体
 * @param clu_port      离合器对应的GPIO的端口
 * @param clu_pin       离合器对应的GPIO的管脚
 * @param vesc_id       本杰明电调的CANID
 * @param dji_board_id  大疆驱动板的驱动板ID
 * @param dji_pmotor_id 大疆驱动板的电机ID(俯仰电机)
 * @param dji_ymotor_id 大疆驱动板的电机ID(云台电机)
 */
void shooter_init(shooter_handle_t sht, const shooter_init_t *init)
{
    sht->init = 0;

    shooter_clutch_init(&sht->clutch, init->clutch_port, init->clutch_pin);
    sht->hcan = init->motor_hcan;
    // sht->vesc_id = init->vesc_id;
    sht->dji_board_id = init->dji_board_id;
    sht->dji_fmotor_id = init->dji_fmotor_id;
    sht->dji_pmotor_id = init->dji_pmotor_id;
    sht->dji_ymotor_id = init->dji_ymotor_id;
    sht->laser = init->laser;
    sht->state = SHOOTER_STATE_START;
    sht->mode = SHOOTER_MODE_DYNAMIC;
    sht->point = 1;

    sht->now_stat.force = 0;
    sht->now_round_cnt = 0;
    sht->now_force_pos = INIT_FORCE_POS;
    sht->now_stat.pitch = 0;
    sht->now_stat.yaw = 0;

    sht->force_pid.Kp = 2.5; // 0.012;
    sht->force_pid.Kd = 0.1; // 0.001;
    // sht->force_pid.KI_P = 0.00015;
    sht->force_pid.last_err = 0;
    // sht->force_pid.i_max_p = 0.20;
    // sht->force_pid.KI_N = 0.00015;
    // sht->force_pid.i_max_n = -0.04;
    // sht->force_pid.i_range_p = 20;
    // sht->force_pid.i_range_n = -10;

    sht->events = osEventFlagsNew(NULL);
    sht->mutex = osMutexNew(NULL);
    sht->queue = osMessageQueueNew(1, sizeof(shooter_status_t), NULL);
    sht->task = osThreadNew(shooter_task, (void *)sht, &shooter_task_attrs);
}

/**
 * @brief 设置射箭参数, 只能在上箭完成后的就绪状态设置
 * 
 * @param sht   射箭机构结构体
 * @param force 拉力大小(编码器值)
 * @param pitch 俯仰大小(俯仰电机位置值)
 * @return int  参数设置成功, 设置成功返回0
 */
int shooter_set_param(shooter_handle_t sht, int force, int pitch)
{
    int ret = -1;
    if (sht->state == SHOOTER_STATE_AIMI)
    {
        static shooter_status_t stat;
        stat.force = force;
        stat.pitch = pitch;
        stat.yaw = LOAD_POS_YAW;
        ret = osMessageQueuePut(sht->queue, &stat, NULL, 0);
    }
    return ret;
}

/**
 * @brief 更新拉力大小, 由ShooterEncoderEx任务调用
 * 
 * @param sht       射箭机构结构体
 * @param force_pos 编码器位置值
 */
void shooter_force_update(shooter_handle_t sht, int force_pos)
{
#ifdef ENCODER_RELATIVE
    int diff_force_pos = -(force_pos - sht->last_force_pos);    // 编码器方向和旋转方向相反
    int now_force = sht->now_stat.force;
    if (diff_force_pos > 180) {
        now_force += (diff_force_pos - 360);
    }
    else if (diff_force_pos < -180) {
        now_force += (diff_force_pos + 360);
    }
    else {
        now_force += diff_force_pos;
    }
    if (osOK == osMutexAcquire(sht->mutex, 0)) {
        sht->now_stat.force = now_force;
        osMutexRelease(sht->mutex);
    }
    sht->last_force_pos = force_pos;
#else
    int diff_force_pos = force_pos - sht->now_force_pos;
    int diff_round_cnt = 0;
    sht->now_force_pos = force_pos;
    if (diff_force_pos > 180)
    {
        diff_round_cnt = -1;
    }
    else if (diff_force_pos < -180)
    {
        diff_round_cnt = 1;
    }
    if (osOK == osMutexAcquire(sht->mutex, 0))
    {
        sht->now_round_cnt += diff_round_cnt;
        osMutexRelease(sht->mutex);
    }
#endif
}

/**
 * @brief 更新俯仰位置, 目前尚未起用
 * 
 * @param sht       射箭机构结构体
 * @param pitch_pos 俯仰电机位置值
 */
void shooter_pitch_update(shooter_handle_t sht, int pitch_pos)
{
    sht->now_stat.pitch = pitch_pos;
}

/**
 * @brief 俯仰电机找零完成, 上电初始化使用
 * 
 * @param sht 射箭机构结构体
 */
void shooter_pitch_homing_ready(shooter_handle_t sht)
{
    UDebug("pitch_homing_ready: %p", sht);
    if (!sht->init)
    {
        UDebug("osEventFlagsSet begin");
        osEventFlagsSet(sht->events, SHOOTER_EVENT_IN_PITCH);
        UDebug("osEventFlagsSet end");
    }
    else
    {
        uprintf("[Error] Shooter pitch homing after initializing\n");
    }
}

/**
 * @brief 云台电机找零完成, 上电初始化使用
 * 
 * @param sht 射箭机构结构体
 */
void shooter_yaw_homing_ready(shooter_handle_t sht)
{
    UDebug("yaw_homing_ready: %p", sht);
    if (!sht->init)
    {
        UDebug("osEventFLagsSet begin");
        osEventFlagsSet(sht->events, SHOOTER_EVENT_IN_YAW);
        UDebug("osEventFlagsSet end");
    }
    else
    {
        uprintf("[Error] Shooter yaw homing after initializing\n");
    }
}

/**
 * @brief 俯仰电机就位
 * 
 * @param sht 射箭机构结构体
 */
void shooter_pitch_ready(shooter_handle_t sht)
{
    if (sht->init)
    {
        sht->now_stat.pitch = sht->tar_stat.pitch;
        // osEventFlagsSet(sht->events, SHOOTER_EVENT_IN_PITCH);
    }
    else
    {
        uprintf("[Error] Shooter pitch arrival before initializing\n");
    }
}

/**
 * @brief 云台电机就位
 * 
 * @param sht 射箭机构结构体
 */
void shooter_yaw_ready(shooter_handle_t sht)
{
    UDebug("yaw_ready: %p", sht);
    if (sht->init)
    {
        UDebug("osEventFlagsSet begin");
        osEventFlagsSet(sht->events, SHOOTER_EVENT_IN_YAW);
        UDebug("osEventFlagsSet end");
    }
    else
    {
        uprintf("[Error] Shooter yaw arrival before initializing\n");
    }
}

void shooter_yaw_arrive(shooter_handle_t sht, float dist)
{
#ifdef TEST_SHOOTER
    uprintf("Arrive!\r\n");
#ifdef TEST_PROCESS
    shooter_set_param(sht, 700, -1500000);
#endif
#else
#ifdef SHOOTER_LASER
    dist = laser_get_dist(sht->laser);
#endif
    //TODO: 根据射箭的点位, 选择拟合公式
    int force, pitch;
    if (sht->point == 0) {
        // 起点I型壶
        force = 860;
        pitch = -600000;
    }
    else if (sht->point == 1) {
        // 射箭点1
        // force = (int)(roundl(dist * 0.14552 + 125.09 + 12));
        force = (int)roundl(dist * 0.0979 + 348.37 + 5);
        force = __max(force, 720);
        force = __min(force, 850);
        pitch = -1000000;
    }
    else {
        // 未知情况
        force = 750;
        pitch = -1000000;
    }
    shooter_set_param(sht, force, pitch);
#endif
}

/**
 * @brief 射箭流程开始, 测试用
 * 
 * @param sht   射箭机构结构体
 * @return int  返回0表示设置成功
 */
int shooter_test_start(shooter_handle_t sht)
{
    UDebug("test_start: %p", sht);
    if (sht->state == SHOOTER_STATE_START)
    {
        UDebug("osEventFlagsSet begin");
        osEventFlagsSet(sht->events, SHOOTER_EVENT_TEST_START);
        UDebug("osEventFlagsSet end");
        return 0;
    }
    else
    {
        return 1;
    }
}

/**
 * @brief 正在取箭, 暂停射箭流程
 * 
 * @param sht   射箭机构结构体
 */
void shooter_pause(shooter_handle_t sht)
{
    osEventFlagsClear(sht->events, SHOOTER_EVENT_OUT_NOTTAKE);
}

/**
 * @brief 取箭完成, 重启射箭流程
 * 
 * @param sht   射箭机构结构体
 */
void shooter_restart(shooter_handle_t sht)
{
    osEventFlagsSet(sht->events, SHOOTER_EVENT_OUT_NOTTAKE);
}

/**
 * @brief 上箭完成, 由上箭机构调用
 * 
 * @param sht   射箭机构结构体
 * @return int  返回0表示设置成功
 */
int shooter_load_ready(shooter_handle_t sht)
{
    UDebug("load_ready: %p", sht);
    if (sht->state == SHOOTER_STATE_LOAD)
    {
        UDebug("osEventFlagsSet begin");
        osEventFlagsSet(sht->events, SHOOTER_EVENT_OUT_LOAD);
        UDebug("osEventFlagsSet end");
        return 0;
    }
    else
    {
        return 1;
    }
}

int shooter_start_aim(shooter_handle_t sht)
{
    if (sht->state == SHOOTER_STATE_READY || sht->state == SHOOTER_STATE_ADJUST || sht->state == SHOOTER_STATE_SHOOT)
    {
        osEventFlagsSet(sht->events, SHOOTER_EVENT_OUT_AIM);
        return 0;
    }
    else
    {
        return 1;
    }
}

/**
 * @brief 发射
 * 
 * @param sht   射箭机构结构体
 * @return int  返回0表示设置成功
 */
int shooter_test_shoot(shooter_handle_t sht)
{
    UDebug("test_shoot: %p", sht);
    if (sht->state == SHOOTER_STATE_SHOOT)
    {
        UDebug("osEventFlagsSet begin");
        osEventFlagsSet(sht->events, SHOOTER_EVENT_OUT_SHOOT);
        UDebug("osEventFlagsSet end");
        return 0;
    }
    else
    {
        return 1;
    }
}

void shooter_set_point(shooter_handle_t sht, int point)
{
    sht->point = point;
}

void shooter_set_mode(shooter_handle_t sht, shooter_mode_t mode)
{
    sht->mode = mode;
}

void shooter_read_yaw(shooter_handle_t sht)
{
    dji_ReadInfo(sht->hcan, sht->dji_board_id, sht->dji_ymotor_id);
}